﻿using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Media.Media3D;

namespace TeaStlShow
{
    public static class ExcuteCuttingPathToAbb
    {
        /// <summary>
        /// 将坐标系的三个正交向量转换为四元数
        /// </summary>
        /// <param name="xAxis">X轴方向向量</param>
        /// <param name="yAxis">Y轴方向向量</param>
        /// <param name="normal">法线方向向量（Z轴）</param>
        /// <returns>表示旋转的四元数</returns>
        public static Quaternion VectorsToQuaternion(Vector3D xAxis, Vector3D yAxis, Vector3D normal)
        {
            // 归一化所有向量
            Vector3D x = NormalizeVector(xAxis);
            Vector3D y = NormalizeVector(yAxis);
            Vector3D z = NormalizeVector(normal);
            // 构建3x3旋转矩阵
            Matrix3D rotationMatrix = new Matrix3D(
                                    x.X, y.X, z.X, 0,
                                    x.Y, y.Y, z.Y, 0,
                                    x.Z, y.Z, z.Z, 0,
                                    0, 0, 0, 1);

            // 将旋转矩阵转换为四元数
            Quaternion answer = MatrixToQuaternion(rotationMatrix);
            //Quaternion testd = new Quaternion(-0.12367, 0.85295,0.43484, 0.26096);//abb的值抄出来测试的
            var anseuar = QuaternionToEulerRadians(answer);
            var yaw = anseuar.yaw * 180 / Math.PI;//EZ
            var pitch = anseuar.pitch * 180 / Math.PI;//EY
            var roll = anseuar.roll * 180 / Math.PI;//EX
            return answer;
        }

        /// <summary>
        /// 将坐标系的三个正交向量转换为欧拉角（弧度）
        /// </summary>
        /// <param name="xAxis">X轴方向向量</param>
        /// <param name="yAxis">Y轴方向向量</param>
        /// <param name="normal">法线方向向量（Z轴）</param>
        /// <returns>欧拉角 (pitch, yaw, roll)</returns>
        public static (double pitch, double yaw, double roll) VectorsToEulerRadians(Vector3D xAxis, Vector3D yAxis, Vector3D normal)
        {
            Quaternion quat = VectorsToQuaternion(xAxis, yAxis, normal);
            return QuaternionToEulerRadians(quat);
        }

        /// <summary>
        /// 将坐标系的三个正交向量转换为欧拉角（角度）
        /// </summary>
        /// <param name="xAxis">X轴方向向量</param>
        /// <param name="yAxis">Y轴方向向量</param>
        /// <param name="normal">法线方向向量（Z轴）</param>
        /// <returns>欧拉角 (pitch, yaw, roll)</returns>
        public static (double pitch, double yaw, double roll) VectorsToEulerDegrees(Vector3D xAxis, Vector3D yAxis, Vector3D normal)
        {
            var (pitch, yaw, roll) = VectorsToEulerRadians(xAxis, yAxis, normal);

            // 转换为角度
            pitch = RadiansToDegrees(pitch);
            yaw = RadiansToDegrees(yaw);
            roll = RadiansToDegrees(roll);

            return (pitch, yaw, roll);
        }

        /// <summary>
        /// 将旋转矩阵转换为四元数
        /// </summary>
        private static Quaternion MatrixToQuaternion(Matrix3D m)
        {
            double trace = m.M11 + m.M22 + m.M33;
            double w, x, y, z;

            if (trace > 0)
            {
                double s = Math.Sqrt(trace + 1.0) * 2;
                w = 0.25 * s;
                x = (m.M32 - m.M23) / s;
                y = (m.M13 - m.M31) / s;
                z = (m.M21 - m.M12) / s;
            }
            else if (m.M11 > m.M22 && m.M11 > m.M33)
            {
                double s = Math.Sqrt(1.0 + m.M11 - m.M22 - m.M33) * 2;
                w = (m.M32 - m.M23) / s;
                x = 0.25 * s;
                y = (m.M12 + m.M21) / s;
                z = (m.M13 + m.M31) / s;
            }
            else if (m.M22 > m.M33)
            {
                double s = Math.Sqrt(1.0 + m.M22 - m.M11 - m.M33) * 2;
                w = (m.M13 - m.M31) / s;
                x = (m.M12 + m.M21) / s;
                y = 0.25 * s;
                z = (m.M23 + m.M32) / s;
            }
            else
            {
                double s = Math.Sqrt(1.0 + m.M33 - m.M11 - m.M22) * 2;
                w = (m.M21 - m.M12) / s;
                x = (m.M13 + m.M31) / s;
                y = (m.M23 + m.M32) / s;
                z = 0.25 * s;
            }

            return Normalized(new Quaternion(x, y, z, w));
        }

        /// <summary>
        /// 四元数转换为欧拉角（弧度）
        /// </summary>
        public static (double pitch, double yaw, double roll) QuaternionToEulerRadians(Quaternion q)
        {
            q = Normalized(q);

            // 计算欧拉角
            double sinr_cosp = 2 * (q.W * q.X + q.Y * q.Z);
            double cosr_cosp = 1 - 2 * (q.X * q.X + q.Y * q.Y);
            double roll = Math.Atan2(sinr_cosp, cosr_cosp);

            double sinp = 2 * (q.W * q.Y - q.Z * q.X);
            double pitch = Math.Abs(sinp) >= 1 ?
                Math.Sign(sinp) * Math.PI / 2 : Math.Asin(sinp);

            double siny_cosp = 2 * (q.W * q.Z + q.X * q.Y);
            double cosy_cosp = 1 - 2 * (q.Y * q.Y + q.Z * q.Z);
            double yaw = Math.Atan2(siny_cosp, cosy_cosp);

            return (pitch, yaw, roll);
        }

        /// <summary>
        /// 四元数转换为欧拉角（角度）
        /// </summary>
        public static (double pitch, double yaw, double roll) QuaternionToEulerDegrees(Quaternion q)
        {
            var (pitch, yaw, roll) = QuaternionToEulerRadians(q);

            pitch = RadiansToDegrees(pitch);
            yaw = RadiansToDegrees(yaw);
            roll = RadiansToDegrees(roll);

            return (pitch, yaw, roll);
        }

        /// <summary>
        /// 验证三个向量是否构成正交坐标系
        /// </summary>
        public static bool ValidateCoordinateSystem(Vector3D xAxis, Vector3D yAxis, Vector3D normal, double tolerance = 1e-6)
        {
            Vector3D x = NormalizeVector(xAxis);
            Vector3D y = NormalizeVector(yAxis);
            Vector3D z = NormalizeVector(normal);

            // 检查正交性
            bool isOrthogonal =
                Math.Abs(Vector3D.DotProduct(x, y)) < tolerance &&
                Math.Abs(Vector3D.DotProduct(x, z)) < tolerance &&
                Math.Abs(Vector3D.DotProduct(y, z)) < tolerance;

            // 检查右手坐标系
            Vector3D cross = Vector3D.CrossProduct(x, y);
            bool isRightHanded = Math.Abs(Vector3D.DotProduct(cross, z)) > (1 - tolerance);

            return isOrthogonal && isRightHanded;
        }

        /// <summary>
        /// 从四元数重建坐标系向量
        /// </summary>
        public static (Vector3D xAxis, Vector3D yAxis, Vector3D normal) QuaternionToVectors(Quaternion q)
        {
            q = Normalized(q);

            // 应用四元数旋转到基准坐标系
            Vector3D baseX = new Vector3D(1, 0, 0);
            Vector3D baseY = new Vector3D(0, 1, 0);
            Vector3D baseZ = new Vector3D(0, 0, 1);

            Matrix3D rotationMatrix = new Matrix3D();
            rotationMatrix.Rotate(q);

            Vector3D xAxis = rotationMatrix.Transform(baseX);
            Vector3D yAxis = rotationMatrix.Transform(baseY);
            Vector3D normal = rotationMatrix.Transform(baseZ);

            return (xAxis, yAxis, normal);
        }

        /// <summary>
        /// 向量归一化
        /// </summary>
        private static Vector3D NormalizeVector(Vector3D vector)
        {
            double length = vector.Length;
            if (length == 0) return new Vector3D(0, 0, 0);
            if (Math.Abs( length - 1)<0.0001)return vector;
            return vector / length;
        }

        /// <summary>
        /// 弧度转角度
        /// </summary>
        private static double RadiansToDegrees(double radians)
        {
            return radians * (180.0 / Math.PI);
        }

        /// <summary>
        /// 角度转弧度
        /// </summary>
        private static double DegreesToRadians(double degrees)
        {
            return degrees * (Math.PI / 180.0);
        }
        /// <summary>
        /// 扩展方法，为Quaternion添加归一化功能
        /// </summary>
        public static Quaternion Normalized(Quaternion q)
        {
            double length = Math.Sqrt(q.W * q.W + q.X * q.X + q.Y * q.Y + q.Z * q.Z);
            if (length == 0) return new Quaternion(0, 0, 0, 1);
            return new Quaternion(q.X / length, q.Y / length, q.Z / length, q.W / length);
        }
    }
}
